#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include "contnav_srvs/initialposeicp.h"

#include <tf_conversions/tf_eigen.h>  // 包含tf::transformTFToEigen函数的头文件
#include <tf/transform_listener.h>    // 包含tf::StampedTransform类和tf::TransformListener类的头文件
#include <Eigen/Core>                 // 包含Eigen::Affine3d类的头文件
#include<signal.h>


#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/common/angles.h>



#include <limits>
#include <fstream>
#include <vector>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/search/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_ros/transforms.h>

#include <auto_lock.h>
#include <mutex>
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudT;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
// 表面法相量的信息
typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
//特征直方图
typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
typedef pcl::search::KdTree<pcl::PointXYZ> SearchMethod;



using namespace std;
using namespace contnav::thread;
//typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> PCLHandler;

/**
 * 加入RANSAC鲁棒姿态估计
 * 点云匹配重定位
 * sukai 2023 03 14
 */


//===================RANSAC鲁棒姿态估计=======点云配准=================================
// 特征点云  获取法线 与 征描述子信息特征直方图特 类
class FeatureCloud{

private:
    // 保存点云数据
    PointCloud::Ptr xyz_;
    // 表面法向量信息
    SurfaceNormals::Ptr normals_;
    // 特征描述子信息
    LocalFeatures::Ptr features_; // 快速点特征直方图
    // 邻域搜索方法
    SearchMethod::Ptr search_method_xyz_; // KDTree

    float normal_radius_;   // 法向量构建时，邻域参考半径
    float feature_radius_;  // 特征描述子构建时，邻域参考半径  要比normal_radius_大一些或相等,不能小
public:
    std::string pcdfilename;

public:
    // 初始化
    // FeatureCloud() : search_method_xyz_(new SearchMethod), normal_radius_(0.02f), feature_radius_(0.02f) {}   //原来的值
    FeatureCloud() : search_method_xyz_(new SearchMethod), normal_radius_(1.2f), feature_radius_(1.21f) {}
  //  FeatureCloud() : search_method_xyz_(new SearchMethod), normal_radius_(2.2f), feature_radius_(2.2f) {}
    PointCloud::Ptr getPointCloud(){
        return xyz_;
    };
    //获取面法向量信息
    SurfaceNormals::Ptr getSurfaceNormals(){
        return normals_;
    };
    //获取特征描述子信息 快速点特征直方图
    LocalFeatures::Ptr getLocalFeatures(){
        return features_;
    };
    // 计算表面法线 xyz_:输入点云  normals_:输出得到表面法向量信息
    void computeSurfaceNormals(){

        // 创建 表面法线堆内存对象  储存表面法线的对象
        normals_ = SurfaceNormals::Ptr(new SurfaceNormals);
        //创建计算表面法线估算的对象   输入:pcl::PointXYZ点  输出：pcl::Normal 法线特征
        pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> nor;
        // 设置输入点云
        nor.setInputCloud(xyz_);

        //邻域搜索方法 KDTree
        nor.setSearchMethod(search_method_xyz_);

        // 设置搜索半经
        nor.setRadiusSearch(normal_radius_);

        // 输出法线
        nor.compute(*normals_);

    }
    // 计算特征描述子 xyz_:输入点云    features_: 输出得到特征描述子信息 快速点特征直方图
    void computeLocalFeatures(){

        // 创建 特征描述子对象  储存特征描述子的对象
        features_ = LocalFeatures::Ptr(new LocalFeatures);
        //创建特征描述子估算的对象   输入:pcl::PointXYZ点  输入: pcl::Normal法线   输出：pcl::features_ 特征描述子
        pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;

        fpfh_est.setInputCloud(xyz_);

        fpfh_est.setInputNormals(normals_);

        //邻域搜索方法 KDTree
        fpfh_est.setSearchMethod(search_method_xyz_);

        fpfh_est.setRadiusSearch(feature_radius_);

        fpfh_est.compute(*features_);

    }
    // 处理输入点云 计算表面法线及计算特征描述子
    void processInput(){
        // 计算表面法线 xyz_:输入点云  normals_:输出得到表面法向量信息
        computeSurfaceNormals();
        // 计算特征描述子 xyz_:输入点云    features_: 输出得到特征描述子信息 快速点特征直方图
        computeLocalFeatures();
    }

    // 通过点云文件路径 加载点云
    void loadInputCloud(const string &pcd_filename){
        // 创建点云
        xyz_ = PointCloud::Ptr(new PointCloud);
        // 通过点云文件路径得到点云数据
        // pcl::io::loadPCDFile(pcd_filename, *xyz_);
        //sukai 2022-10-20
        if(pcl::io::loadPCDFile(pcd_filename, *xyz_)==-1){
            PCL_ERROR("Couldn't read file test_pcd.pcd\n");
            std::cout << "-----------请检查 output/object_templates.txt 是否有对应的点云模板-------------pcd_filename: " <<pcd_filename<< std::endl;
            return ;
        }
        // 处理输入点云得到: 表面法线normals_ 及 特征描述子features_
        processInput();
    }

    void setInputCloud(PointCloud::Ptr cloud) {

        //设置输入点云
        xyz_ = cloud;
        // 处理输入点云
        processInput();
    }

};

/**
 *
 *  匹配器，执行模板匹配
 * 此对象专门用来进行刚体变换匹配
 * 匹配成功的模板点云 + 模板变换到目标的变换矩阵
 */
class TemplateAligment{

private:
    vector<FeatureCloud> templates_;
    FeatureCloud target_;
    // include <pcl/registration/ia_ransac.h>  ,SampleConsensusInitialAlignment这个可以有效的排除异常的情况
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
    //pcl::SampleConsensusPrerejective<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
    // 最小采样距离
    float min_sample_distance_;
    // 最大的响应距离,如果发现两个点太远了,认为它没有在模型内部
    float max_correspondence_distance_;
    // 最大迭代次数
    float max_iterations_;

public:

    TemplateAligment()



        : min_sample_distance_(0.01f),  //sukai
                // max_correspondence_distance_(2.0f),
                // max_correspondence_distance_(1.0f*0.99f),//1.0f*0.99f
                max_correspondence_distance_(10.0f),//1.0f*0.99f
                max_iterations_(5000) {
        sac_ia_.setTransformationEpsilon(0.0001);
        sac_ia_.setMinSampleDistance(min_sample_distance_);
        sac_ia_.setMaxCorrespondenceDistance(max_correspondence_distance_);
        sac_ia_.setMaximumIterations(max_iterations_);



    }
    void addTemplateCloud(FeatureCloud &cloud) {
        templates_.push_back(cloud);
    }
    void setTargetCloud(FeatureCloud &target_cloud){
        target_ = target_cloud;
        sac_ia_.setInputTarget(target_cloud.getPointCloud());
        sac_ia_.setTargetFeatures(target_cloud.getLocalFeatures());
    };
    struct Result {
        float fitness_score;
        int hasConverged;

        Eigen::Matrix4f transformation;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    /**
     * 3.
     * 核心代码:
     * 执行匹配，并将结果设置给 result
     * @param result
     * @param template_cloud
     */
    //todo   ======================  执行匹配核心代码 ==============================
    void align(FeatureCloud &template_cloud, TemplateAligment::Result &result){
        sac_ia_.setInputSource(template_cloud.getPointCloud());
        sac_ia_.setSourceFeatures(template_cloud.getLocalFeatures());

        PointCloud registration_output;
        sac_ia_.align(registration_output);

        result.fitness_score = (float) sac_ia_.getFitnessScore(max_correspondence_distance_);
        result.transformation = sac_ia_.getFinalTransformation();
        result.hasConverged = sac_ia_.hasConverged();


    };
       // pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 T_prev = Eigen::Matrix4f::Identity();
    void align(FeatureCloud &template_cloud, TemplateAligment::Result &result,pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 &T_prev){
        sac_ia_.setInputSource(template_cloud.getPointCloud());
        sac_ia_.setSourceFeatures(template_cloud.getLocalFeatures());

        PointCloud registration_output;
        sac_ia_.align(registration_output,T_prev);

        result.fitness_score = (float) sac_ia_.getFitnessScore(max_correspondence_distance_);
        result.transformation = sac_ia_.getFinalTransformation();
        result.hasConverged = sac_ia_.hasConverged();


    };

    void alignAll(std::vector<Result, Eigen::aligned_allocator<Result>>& results,pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 &T_prev){//Eigen下提供了，数据分配符，数据对齐aligned_allocator
        results.resize(templates_.size());
        for (int i = 0; i < templates_.size(); ++i) {
            /** 3.
              * 核心代码:
              * 执行匹配，并将结果设置给 result
              * @param result
              * @param template_cloud
              */
            align(templates_[i], results[i],T_prev);
        }
    };

    /**
     *  1.
     * 在templates_中查找与target_匹配结果最佳的点云索引
     * 根据匹配分数判断（越低越好）
     * 查找最优解
     */
    int findBestAligment(TemplateAligment::Result &result,pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 &T_pre){
        std::vector<Result, Eigen::aligned_allocator<Result>> results;
        alignAll(results,T_pre);

        int best_template_index = 0;
        float lowest_score = results[0].fitness_score;
        //排序
        for (int i = 0; i < results.size(); ++i) {
            Result &result = results[i];
            // 取分数最小的
            if (result.fitness_score < lowest_score) {
                lowest_score = result.fitness_score;
                best_template_index = i;
            }
        }
        result = results[best_template_index];
        return best_template_index;
    };
};




/**
 * 匹配
 */
class LaserScanToPointCloud
{
public:
    ~LaserScanToPointCloud() {
        initialposeWithIcpfig=false;
        num=-1;
        prev_score=std::numeric_limits<float>::max ();;

    }
    LaserScanToPointCloud() : nh_{"~"}
    {
        T_prev = Eigen::Matrix4f::Identity();//RANSAC
       // scan_sub_ = nh_.subscribe("/scan", 1, &LaserScanToPointCloud::scanCallback, this);
        initial_pose_pub = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10);
        occupancyGridToCloudpub = nh_.advertise<sensor_msgs::PointCloud>("/occupancyGridToCloud", 1);//输出地图点云
        pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud>("/scan_to_pointcloud", 1);//输出雷达的点在map下的坐标点云
        pointcloudIcpndt_pub_ = nh_.advertise<sensor_msgs::PointCloud>("/out_icp_ndt_pointcloud", 1);//输出输出匹配好的点云。局部图

        initialposeWithIcpServer = nh_.advertiseService<contnav_srvs::initialposeicp::Request,contnav_srvs::initialposeicp::Response>("/initialposeWithIcp", boost::bind(&LaserScanToPointCloud::callback_initialposeWithIcpServerFun,this,_1,_2));

        //获取地图点云
        getoccupancyGridToCloud();


    }


    void  thread_initialpose() {
        //获取地图点云
        getoccupancyGridToCloud();


        ros::topic::waitForMessage<sensor_msgs::LaserScan>("/scan", ros::Duration(5));
        try {

         while((num>0||num==-2)&&initialposeWithIcpfig){

             if(num!=-2)
                num--;

             boost::shared_ptr<sensor_msgs::LaserScan  const>    scan_msg = ros::topic::waitForMessage<sensor_msgs::LaserScan>("/scan", ros::Duration(0.5));
             if (scan_msg)
             {
                 AutoLock lock(&m_mutex);
                 lock.try_lock();

                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
                 for (size_t i = 0; i < scan_msg->ranges.size(); ++i)
                 {
                     double range = scan_msg->ranges[i];
                     if (std::isnan(range) || range <= scan_msg->range_min || range >= scan_msg->range_max)
                     {
                         continue;
                     }
                     double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
                     pcl::PointXYZ point;
                     point.x = range * std::cos(angle);
                     point.y = range * std::sin(angle);
                     point.z = 0.0;
                     cloud->push_back(point);
                 }


                 if (cloud->empty()||map_point_cloud->empty()){

                     std::cout<<"===================未获取到地图或雷达数据==========================="<<std::endl;
                     return;
                 }


                 tf::StampedTransform transform;
                 try
                 {


                     tf_listener_.waitForTransform("/map", scan_msg->header.frame_id, ros::Time(0), ros::Duration(1.0));
                     tf_listener_.lookupTransform("/map", scan_msg->header.frame_id, ros::Time(0), transform);
                 }
                 catch (const tf::TransformException& ex)
                 {
                    // m_.unlock();
                     ROS_ERROR("%s", ex.what());
                     return;
                 }
                 Eigen::Affine3d eigen_transform;
                 tf::transformTFToEigen(transform, eigen_transform);
                 pcl::transformPointCloud(*cloud, *cloud, eigen_transform);



                 sensor_msgs::PointCloud2 pointcloud2_msg;
                 sensor_msgs::PointCloud pointcloud_msg;
                 pcl::toROSMsg(*cloud, pointcloud2_msg);
                 sensor_msgs::convertPointCloud2ToPointCloud(pointcloud2_msg, pointcloud_msg);

                 pointcloud_msg.header.frame_id = "map";
                 pointcloud_msg.header.stamp = scan_msg->header.stamp;
                 pointcloud_pub_.publish(pointcloud_msg);

                 // Save point cloud data to file
                 if(savepcd)
                     pcl::io::savePCDFileASCII("input_cloud.pcd", *cloud);
                 float score;
                 float scoreRANSAC;
                 Eigen::Matrix4d identity_transform;
                 pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 matrix4transform;

                 bool fig  =true;
                 if(isndtfig==1) {//icp

                     //用 icp
                     fig  =  iterative_closest_point(map_point_cloud,cloud,matrix4transform,T_prev,score);//icp
                     identity_transform = Eigen::Matrix4d::Identity();
                     for (int i = 0; i < 4; i++) {
                         for (int j = 0; j < 4; j++) {
                             identity_transform(i, j) = matrix4transform(i, j);
                         }
                     }

                 }else if(isndtfig==2){//ndt

                     //ndt
                     fig  = normal_distributions_transform(map_point_cloud,cloud,matrix4transform,T_prev,score);//ndt


                     identity_transform = Eigen::Matrix4d::Identity();
                     for (int i = 0; i < 4; i++) {
                         for (int j = 0; j < 4; j++) {
                             identity_transform(i, j) = matrix4transform(i, j);
                         }
                     }

                 }else{

                     //todo RANSAC 鲁棒姿态估计  scoreRANSAC
                     fig= RANSAC_transform(map_point_cloud,cloud,identity_transform,T_prev,score);
                     matrix4transform = Eigen::Matrix4f::Identity();
                     Eigen::Map<Eigen::Matrix4f>(matrix4transform.data()) = identity_transform.cast<float>();


                 }



                 //--------------------
                 tf::StampedTransform transform_base_link;
                 try
                 {

                     tf_listener_.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(0.1));
                     tf_listener_.lookupTransform("/map", "/base_link", ros::Time(0), transform_base_link);
                 }
                 catch (const tf::TransformException& ex)
                 {
                   //  m_.unlock();
                     ROS_ERROR("%s", ex.what());
                     return;
                 }
                 // Assume we have a tf::StampedTransform object called transform
                 Eigen::Affine3d affine;
                 tf::transformTFToEigen(transform_base_link, affine);
                 Eigen::Matrix4d base_linkmatrix = affine.matrix();
                 //  Eigen::Matrix4f matrix4f = base_linkmatrix.cast<float>();

                 Eigen::Matrix4d  base_linkmatrix_new =identity_transform*base_linkmatrix;

                 Eigen::Vector3d pos = base_linkmatrix_new.block<3,1>(0,3);
                 Eigen::Matrix3d rot = base_linkmatrix_new.block<3,3>(0,0);

                 //=======================================================
                 geometry_msgs::PoseWithCovarianceStamped mapTuwbpose;
                 mapTuwbpose.header.stamp=ros::Time::now();
                 mapTuwbpose.header.frame_id = "map" ;
                 mapTuwbpose.pose.pose.position.x=pos(0);
                 mapTuwbpose.pose.pose.position.y=pos(1);//z: pos(2)

                 mapTuwbpose.pose.covariance[0] = 0.25;
                 mapTuwbpose.pose.covariance[6 * 1 + 1] = 0.25;
                 mapTuwbpose.pose.covariance[6 * 5 + 5] = 0.06853891945200942;

                 // 将姿态信息填充到pose_msg消息中
                 Eigen::Quaterniond quat(rot);

                 mapTuwbpose.pose.pose.orientation.z=quat.z();
                 mapTuwbpose.pose.pose.orientation.w=quat.w();

                 if ( score < prev_score ) {//&& score > min_score 取匹配得分最低的数据进行重定位；
                     //  if (score>0 && score< 0.02 ) {//&& score > min_score
                     prev_score = score;
                     T_prev=matrix4transform;

                     initial_pose_pub.publish(mapTuwbpose);
                 }


                 PointCloudT::Ptr output_cloud(new PointCloudT);


                 PointCloudT::Ptr input_cloud1(cloud);
                 pcl::transformPointCloud(*input_cloud1, *output_cloud, matrix4transform);

                 sensor_msgs::PointCloud2 output_cloud2_msg;
                 sensor_msgs::PointCloud output_cloud_msg;
                 pcl::toROSMsg(*output_cloud, output_cloud2_msg);
                 sensor_msgs::convertPointCloud2ToPointCloud(output_cloud2_msg, output_cloud_msg);

                 output_cloud_msg.header.frame_id = "map";
                 output_cloud_msg.header.stamp = scan_msg->header.stamp;
                 pointcloudIcpndt_pub_.publish(output_cloud_msg);



             }//     if (scan_msg)

             else{
                 std::cout <<"no scan"  << std::endl;

             }

         }



        } catch (exception e) {
            cout << e.what() << endl;

        }




    }
    bool callback_initialposeWithIcpServerFun(contnav_srvs::initialposeicp::Request &request  , contnav_srvs::initialposeicp::Response &response) {



        bool fig =true;
        try {


            /**
             * 接收用户指令
             ros::ServiceClient    initialposeicpClient = node_handle_.serviceClient<contnav_srvs::initialposeicp::Request, contnav_srvs::initialposeicp::Response>("/chargeMove");
           string request_type  initialpose：重定位
            int32   isndtfig  1:icp   2:ndt   3.(SAC-IA)
            int32   num n个循环匹配, -2:一直循环定位
            ---
            string result
            string message

            */

            //todo 接收用户指令：
            if(request.request_type=="initialpose" ){//重定位
                isndtfig=request.isndtfig;
                num=request.num;
                prev_score=std::numeric_limits<float>::max ();;
                initialposeWithIcpfig=true;

                //执行多线程
                boost::thread parse_thread(boost::bind(&LaserScanToPointCloud::thread_initialpose, this));
            }else if(request.request_type=="stop"){
                initialposeWithIcpfig=false;
                num=-1;

            }

        }catch (std::exception e){

            cout<<  e.what()<<endl;
            ROS_ERROR("callback_SqlOperationFun()->exception ");

        }

        if(fig){
            response.result="ok";
        }else{
            response.result="error";
        }

        return true;
    }


    //获取地图点云
    void getoccupancyGridToCloud(){
        map_point_cloud =pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
        boost::shared_ptr<nav_msgs::OccupancyGrid  const>  occupancy_grid_msg = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("/map", ros::Duration(2));///uwb/data
        if(occupancy_grid_msg){
            int width = occupancy_grid_msg->info.width;
            int height = occupancy_grid_msg->info.height;
            float resolution = occupancy_grid_msg->info.resolution;
            std::vector<int8_t> data = occupancy_grid_msg->data;
            float origin_x = occupancy_grid_msg->info.origin.position.x;
            float origin_y = occupancy_grid_msg->info.origin.position.y;
            // Convert occupancy grid data to point cloud data
            for (int i = 0; i < height; i++) {
                for (int j = 0; j < width; j++) {

                    if (data[i * width + j]== -1) continue;
                    if (data[i * width + j] > 50) {

                  //  if (data[i * width + j] == 100) {
                        float point_x  = j * resolution + origin_x;
                        float point_y  = i * resolution + origin_y;
                        float point_z = 0.0;
                        map_point_cloud->push_back(pcl::PointXYZ(point_x, point_y, point_z));
                    }
                }
            }



            sensor_msgs::PointCloud2 pointcloud2_msg;
            sensor_msgs::PointCloud pointcloud_msg;
            pcl::toROSMsg(*map_point_cloud, pointcloud2_msg);
            sensor_msgs::convertPointCloud2ToPointCloud(pointcloud2_msg, pointcloud_msg);


            pointcloud_msg.header.frame_id = occupancy_grid_msg->header.frame_id;
            pointcloud_msg.header.stamp = ros::Time::now();

            // Publish point cloud message

            occupancyGridToCloudpub.publish(pointcloud_msg);


            // Save point cloud data to file
            if(savepcd)
                pcl::io::savePCDFileASCII("target_cloud.pcd", *map_point_cloud);

        }else{

            std::cout<<"===================未获取到地图==========================="<<std::endl;
        }
    }

    //icp
    bool iterative_closest_point(PointCloudT::Ptr &target_cloud,PointCloudT::Ptr &input_cloud,pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix4,pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 &T_prev,float &score){

        bool fig =true;

         pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

        icp.setInputSource(input_cloud);//input_cloud

        icp.setInputTarget(target_cloud);//target_cloud

        pcl::PointCloud<pcl::PointXYZ> final;






        icp.setMaximumIterations(1);
        icp.setTransformationEpsilon(1);
        icp.setMaxCorrespondenceDistance(10000);
        // 输出原点云经过配准变换之后的点云
        icp.align(final,T_prev);//T_prev
        score=icp.getFitnessScore();//匹配分数score

        std::cout << "=========*******************************************=====icp=========*******************************************============" << icp.hasConverged() << std::endl; // 值==1时，表示结果收敛了（true）
        std::cout << "结果是否收敛：" << icp.hasConverged() << std::endl; // 值==1时，表示结果收敛了（true）
        std::cout << "匹配分数（每一组对应点距离）：" << icp.getFitnessScore() << std::endl;
        matrix4 = icp.getFinalTransformation();
        std::cout << "matrix:\n" << matrix4 << std::endl;//
/**

            1           0            0          0.7
            0           1            0            0
            0           0            1            0
            0           0            0            1

 */

        //  if(icp.getFitnessScore()>0.000741124)//0.000441124
        if(icp.getFitnessScore()>0.100741124)//0.000441124
            fig =false;

        return fig;
    }


    //ndt
    bool normal_distributions_transform(PointCloudT::Ptr &target_cloud,PointCloudT::Ptr &input_cloud,pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix4,pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 &T_prev,float &score){

        bool fig =true;

        // 使用NDT对象初始化pdfs（概率密度函数块）
        pcl::NormalDistributionsTransform<PointT, PointT> ndt;
        ndt.setTransformationEpsilon(0.00001);
        //ndt.setStepSize(0.1);
        ndt.setStepSize(0.2);
        ndt.setResolution(1);
        //ndt.setMaximumIterations(35);
        ndt.setMaximumIterations(10000);
        // ndt.setInputSource(filtered_cloud);
        ndt.setInputSource(input_cloud);//input_cloud
        ndt.setInputTarget(target_cloud);//target_cloud

        // 设置一些初始化评估位姿(粗配准)
        Eigen::AngleAxisf init_rotation(pcl::deg2rad(0.001f) ,Eigen::Vector3f::UnitZ()); // 绕Z轴旋转弧度 40.0f
        Eigen::Translation3f init_translation(0, 0, 0);//1.79387, 0, 0
        Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

        // 执行配准，准备容器（配准后原点云变换后的点云）
        PointCloudT::Ptr transformed_cloud(new PointCloudT);

        ndt.align(*transformed_cloud,T_prev);//init_guess T_prev

        score=ndt.getFitnessScore();
        //----------------------------------------
        std::cout << "======*****************=====ndt=========****************===========结果是否收敛：\n" << ndt.hasConverged()

                  << " 匹配分数：" << ndt.getFitnessScore() << std::endl;  //0.000441124
        std::cout << "变换矩阵：\n" << ndt.getFinalTransformation() << std::endl;

        matrix4 = ndt.getFinalTransformation();
        //  if(ndt.getFitnessScore()>0.000741124)//0.000441124
        if(ndt.getFitnessScore()>1.100741124)//0.000441124
            fig =false;


        return fig;
    }


    //RANSAC 鲁棒姿态估计
    bool RANSAC_transform(PointCloudT::Ptr &target_cloud,PointCloudT::Ptr &input_cloud,Eigen::Matrix4d  &eigen_matrixd ,pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 &T_prev,float &score){

        bool fig =true;
        //======================================
        FeatureCloud targetFeatureCloudcloud;
        targetFeatureCloudcloud.setInputCloud(target_cloud);
        //------------------

        FeatureCloud templateFeatureCloud_cloud;
        templateFeatureCloud_cloud.setInputCloud(input_cloud);
        //------------------
        TemplateAligment template_align;
        template_align.setTargetCloud(targetFeatureCloudcloud);

        template_align.addTemplateCloud(templateFeatureCloud_cloud);

        TemplateAligment::Result result;

        int best_index = template_align.findBestAligment(result,T_prev);//返回最佳模板配准结果索引

        score=result.fitness_score;
        std::cout << "*********** RANSAC==*******=======结果是否收敛：\n" << result.hasConverged<< std::endl;

        std::cout << "结果是否收敛： " << result.hasConverged << std::endl;

        std::cout << "匹配分数score: " << result.fitness_score << " index: " << best_index << std::endl;
        std::cout << "变换矩阵：\n" << result.transformation << std::endl;
        Eigen::Matrix4f   transformationMatrix4f = result.transformation;

         eigen_matrixd = transformationMatrix4f.cast<double>();
        std::cout<<"eigen_matrix: "<<eigen_matrixd<<std::endl;

            if(result.fitness_score>1.009)
            fig =false;

        return fig;
    }



private:
    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
    {
        AutoLock lock(&m_mutex);
        lock.try_lock();

        if(getoccupancyGridToCloudfig){

            getoccupancyGridToCloud();
            getoccupancyGridToCloudfig=false;
        }



        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        for (size_t i = 0; i < scan_msg->ranges.size(); ++i)
        {
            double range = scan_msg->ranges[i];
            if (std::isnan(range) || range <= scan_msg->range_min || range >= scan_msg->range_max)
            {
                continue;
            }
            double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
            pcl::PointXYZ point;
            point.x = range * std::cos(angle);
            point.y = range * std::sin(angle);
            point.z = 0.0;
            cloud->push_back(point);
        }


        if (cloud->empty()||map_point_cloud->empty()){

            std::cout<<"===================未获取到地图或雷达数据==========================="<<std::endl;
            return;
        }



        tf::StampedTransform transform;
        try
        {

            tf_listener_.waitForTransform("/map", scan_msg->header.frame_id, ros::Time(0), ros::Duration(3.0));
            tf_listener_.lookupTransform("/map", scan_msg->header.frame_id, ros::Time(0), transform);
        }
        catch (const tf::TransformException& ex)
        {
            ROS_ERROR("%s", ex.what());
            return;
        }
        Eigen::Affine3d eigen_transform;
        tf::transformTFToEigen(transform, eigen_transform);
        pcl::transformPointCloud(*cloud, *cloud, eigen_transform);





        sensor_msgs::PointCloud2 pointcloud2_msg;
        sensor_msgs::PointCloud pointcloud_msg;
        pcl::toROSMsg(*cloud, pointcloud2_msg);
        sensor_msgs::convertPointCloud2ToPointCloud(pointcloud2_msg, pointcloud_msg);

        pointcloud_msg.header.frame_id = "map";
        pointcloud_msg.header.stamp = scan_msg->header.stamp;
        pointcloud_pub_.publish(pointcloud_msg);

        // Save point cloud data to file
        if(savepcd)
            pcl::io::savePCDFileASCII("input_cloud.pcd", *cloud);

        float score;
        float scoreRANSAC;
        Eigen::Matrix4d identity_transform;
        pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 matrix4transform;

        bool fig  =true;
        if(isndtfig==1) {

            //用 icp
             fig  =  iterative_closest_point(map_point_cloud,cloud,matrix4transform,T_prev,score);//icp


            identity_transform = Eigen::Matrix4d::Identity();

            for (int i = 0; i < 4; i++) {
                for (int j = 0; j < 4; j++) {
                    identity_transform(i, j) = matrix4transform(i, j);
                }
            }

        }else if(isndtfig==2){

                //ndt
                fig  = normal_distributions_transform(map_point_cloud,cloud,matrix4transform,T_prev,score);//ndt

                identity_transform = Eigen::Matrix4d::Identity();

                for (int i = 0; i < 4; i++) {
                    for (int j = 0; j < 4; j++) {
                        identity_transform(i, j) = matrix4transform(i, j);
                    }
                }


        }else{

            fig= RANSAC_transform(map_point_cloud,cloud,identity_transform,T_prev,score);
             matrix4transform = Eigen::Matrix4f::Identity();
            Eigen::Map<Eigen::Matrix4f>(matrix4transform.data()) = identity_transform.cast<float>();

        }

        std::cout <<"identity_transform： \n" <<identity_transform << std::endl;


        tf::StampedTransform transform_base_link;
        try
        {    //base_scan


            tf_listener_.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(0.1));
            tf_listener_.lookupTransform("/map", "/base_link", ros::Time(0), transform_base_link);
        }
        catch (const tf::TransformException& ex)
        {
            ROS_ERROR("%s", ex.what());
            return;
        }
        // Assume we have a tf::StampedTransform object called transform
        Eigen::Affine3d affine;
        tf::transformTFToEigen(transform_base_link, affine);
        Eigen::Matrix4d base_linkmatrix = affine.matrix();
        //  Eigen::Matrix4f matrix4f = base_linkmatrix.cast<float>();

        Eigen::Matrix4d  base_linkmatrix_new =identity_transform*base_linkmatrix;

        Eigen::Vector3d pos = base_linkmatrix_new.block<3,1>(0,3);
        Eigen::Matrix3d rot = base_linkmatrix_new.block<3,3>(0,0);

        //=======================================================
        geometry_msgs::PoseWithCovarianceStamped mapTuwbpose;
        mapTuwbpose.header.stamp=ros::Time::now();
        mapTuwbpose.header.frame_id = "map" ;
        mapTuwbpose.pose.pose.position.x=pos(0);
        mapTuwbpose.pose.pose.position.y=pos(1);//z: pos(2)

        mapTuwbpose.pose.covariance[0] = 0.25;
        mapTuwbpose.pose.covariance[6 * 1 + 1] = 0.25;
        mapTuwbpose.pose.covariance[6 * 5 + 5] = 0.06853891945200942;

        // 将姿态信息填充到pose_msg消息中
        Eigen::Quaterniond quat(rot);

        mapTuwbpose.pose.pose.orientation.z=quat.z();
        mapTuwbpose.pose.pose.orientation.w=quat.w();
      if ( score < prev_score ) {//&& score > min_score

            prev_score = score;
            T_prev=matrix4transform;

           initial_pose_pub.publish(mapTuwbpose);
       }


        PointCloudT::Ptr output_cloud(new PointCloudT);
//        pcl::transformPointCloud(*cloud, *output_cloud, matrix4transform);

        PointCloudT::Ptr input_cloud1(cloud);
        pcl::transformPointCloud(*input_cloud1, *output_cloud, matrix4transform);

        // 将pcl点云转换为PointCloud消息并发布
        sensor_msgs::PointCloud2 output_cloud2_msg;
        sensor_msgs::PointCloud output_cloud_msg;
        pcl::toROSMsg(*output_cloud, output_cloud2_msg);
        sensor_msgs::convertPointCloud2ToPointCloud(output_cloud2_msg, output_cloud_msg);

        output_cloud_msg.header.frame_id = "map";
        output_cloud_msg.header.stamp = scan_msg->header.stamp;
        pointcloudIcpndt_pub_.publish(output_cloud_msg);




    }
    Mutex m_mutex;
    ros::NodeHandle nh_;
    ros::Subscriber scan_sub_;
    ros::Publisher pointcloud_pub_,pointcloudIcpndt_pub_,initial_pose_pub,occupancyGridToCloudpub;
    tf::TransformListener tf_listener_;
    //  pcl::PointCloud<pcl::PointXYZ>::Ptr map_point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr map_point_cloud;
    bool  getoccupancyGridToCloudfig = true;

    bool savepcd=false;//保存点云

    pcl::registration::TransformationEstimation<pcl::PointXYZ, pcl::PointXYZ>::Matrix4 T_prev;
    float prev_score = std::numeric_limits<float>::max ();
    float min_score = 0.01;


    mutex m_ ;
public:
    int isndtfig=3;//启动 icp
    int   num=5;
    bool initialposeWithIcpfig=true;
    ros::ServiceServer initialposeWithIcpServer ;


};

LaserScanToPointCloud *currency = NULL;
//ctrl + c
void mySigintHandler(int sig) {
    std::cout << "ctrl c  sig: " << sig << std::endl;
    if (currency != NULL)
        currency->initialposeWithIcpfig=false;
        currency->num=-1;
        delete currency;

    ros::shutdown();
}
// rosrun pointCloudInitialpose  initialposeWithIcpAndNdtAndSACIA
int main(int argc, char** argv)
{
    ros::init(argc, argv, "initialposeWithIcpAndNdtAndSACIA");
    ros::NodeHandle nh;
    signal(SIGINT, mySigintHandler);

    currency =new LaserScanToPointCloud();


    ros::spin();
    return 0;
}

